#include "../User/Component/AHRS.h"

#include <math.h>

#define MACHINE_ZERO_ROLL (-2.28f)
#define MACHINE_ZERO_PITCH (0.0f)
#define IMU_RANGE (65520.0f)
#define IMU_ACCEL_COMPANY (16.0f)
#define IMU_GYRO_COMPANY (2000)

void AHRS_PraseIMU(AHRS_Accel_t *accel, AHRS_Gyro_t *gyro, int16_t *imu_accel,
                   int16_t *imu_gyro) {
  accel->x = (float)imu_accel[0] / IMU_RANGE * IMU_ACCEL_COMPANY;
  accel->y = (float)imu_accel[1] / IMU_RANGE * IMU_ACCEL_COMPANY;
  accel->z = (float)imu_accel[2] / IMU_RANGE * IMU_ACCEL_COMPANY;

  gyro->x = imu_gyro[0] / IMU_RANGE * IMU_GYRO_COMPANY;
  gyro->y = imu_gyro[1] / IMU_RANGE * IMU_GYRO_COMPANY;
  gyro->z = imu_gyro[2] / IMU_RANGE * IMU_GYRO_COMPANY;
}

void AHRS_PraseAngle(AHRS_Angle_t *euler, AHRS_Accel_t *accel) {
  euler->Pitch = atan2(accel->x, accel->z) * 180 / PI + MACHINE_ZERO_PITCH;
  euler->Roll = atan2(accel->y, accel->z) * 180 / PI + MACHINE_ZERO_ROLL;
}
